#include "siw_control.h"

/** Contains the current baud rate and other settings of the virtual serial port. While this demo does not use
 *  the physical USART and thus does not use these settings, they must still be retained and returned to the host
 *  upon request or the host will assume the device is non-functional.
 *
 *  These values are set by the host via a class-specific request, however they are not required to be used accurately.
 *  It is possible to completely ignore these value or use other settings as the host is completely unaware of the physical
 *  serial link characteristics and instead sends and receives data in endpoint streams.
 */

// ASCII 'SRV'
#define Servo 0x535256

// ASCII 'TMP'
#define Temp 0x544D50

// ASCII 'HTR'
#define Heater 0x485452

// ASCII 'CAM'
#define Camera 0x43414D

// ASCII 'LAP'
#define Laptop 0x4C4150

// ASCII 'USB'
#define USB 0x555342

// Variables used for temperature sensor (input capture interrupt)
volatile unsigned int current, prev, period;
volatile int num_av = 30;
volatile int count = 0;
volatile int settle = 10;
volatile double total = 0;
volatile int sleeping = 0;
volatile int usb_timer_running = 0;
volatile int usb_timer_value = 0;

// 61 = 1 second
#define timer_secs 61
#define timer_mins 3662
volatile int usb_timer_expire = 75*timer_secs;

uint8_t PushButton_GetStatus(void)
{
  return !(uint8_t)(PIND & _BV(PD7));
}

int main(void)
{
  SetupHardware();
  
  DDRB |= 0b00000100; // Set Cam switch to output
  DDRB |= 0b00100100; // Set Heater switch to output
  DDRC |= 0b00100000; // Set USB switch to output
  DDRC |= 0b00010000; // Set Laptop switch to output
  
  LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
  sei();

  DDRB |= PB1 | PB3;

  for (;;)
  {
    CDC_Task();
    USB_USBTask();
//     if (sleeping) LEDs_FlashLEDs(LED_BLUE);
  }
}

/** Function to manage CDC data transmission and reception to and from the host. */
void CDC_Task(void)
{
	/* Device must be connected and configured for the task to run */
	if (USB_DeviceState != DEVICE_STATE_Configured) 
	{
		if (sleeping==0) start_USB_timer();
		return;
	}
	
	/* Select the Serial Rx Endpoint */
	Endpoint_SelectEndpoint(CDC_RX_EPNUM);
	
	if (Endpoint_IsOUTReceived())
	{
		cli();
/*		uint8_t input = Endpoint_Read_Byte();*/
		uint32_t input_dword = Endpoint_Read_DWord_BE();
		uint32_t command = input_dword >> 8;
		uint8_t value = input_dword & 0x000000ff;
		
		switch (command)
		{
			case Temp:
			{
// 				start_USB_timer();
				double temp = Get_Temp();
				char buffer [30];
				sprintf(buffer,"%x\n", *(unsigned int *) 0xd0);
				//buffer = 'Tom Wins!';
				char* message = buffer;
				Send_String(message);
			}
				break;
				
				
			case Servo:
			{
				double angle = 2.0*(value - 15);
				char buffer [30];
				sprintf(buffer,"Turning to %.0f degrees",angle);
				char* message = buffer;
				Send_String(message);
				Turn_Servo(angle);
			}
				break;

			case Heater:
			{
				uint8_t value_temp = value - 48;
				char buffer [50];
				// Set PB2 as output
				DDRB |= 0b00100000;
				switch (value_temp)
				{
					case 1:
						sprintf(buffer,"Turned heater on (%i)",value_temp);
						PORTB |= 0b00100000;
						break;
					case 0:
						sprintf(buffer,"Turned heater off (%i)",value_temp);
						PORTB &= ~0b00100000;
						break;
					default:
						sprintf(buffer,"Invalid command (use 'H0' or 'H1')");
						break;
				}
				char* message = buffer;
				Send_String(message);
			}
				break;

			case USB:
			{
				LEDs_FlashLEDs(LED_RED);
				_delay_ms(100);
				LEDs_FlashLEDs(LED_RED);
				_delay_ms(100);
				LEDs_FlashLEDs(LED_RED);
				_delay_ms(100);
				LEDs_FlashLEDs(LED_RED);
				DDRC |= 0b00100000; // set PC5 (Bit5 = 1) as output 
				char buffer [30];
				sprintf(buffer,"Restarting USB");
				PORTC |= 0b00100000; 
				_delay_ms(1500);
				PORTC &= ~0b00100000;
				char* message = buffer;
				Send_String(message);
			}
				break;
				
			case Laptop:
			{
				uint8_t value_sleep = value - 48;
				char buffer [30];
				switch (value_sleep) 
				{
					case 1:
						sleeping = 1;
						sprintf(buffer,"Switching sleeping to %i",sleeping);
						break;
					case 0:
						sleeping = 0;
						sprintf(buffer,"Switched sleeping to %i",sleeping);
						break;
					default:
						sprintf(buffer,"Invalid command (use 'LAP0' or 'LAP1')");
						break;
				}
// 				DDRC |= 0b00100000; // set PC5 (Bit5 = 1) as output 
				
				// 				PORTC |= 0b00100000; 
// 				_delay_ms(1000);
// 				PORTC &= ~0b00100000;
				char* message = buffer;
				Send_String(message);
			}
				break;
				
			case Camera:
			{
				char buffer [30];
				// Set PB2 as output
				DDRB |= 0b00000100;
				
				PORTB |= 0b00000100;
				_delay_ms(900);
				PORTB &= ~0b00000100;
				
				sprintf(buffer,"Switching camera state");
				char* message = buffer;
				Send_String(message);
			}
				break;
			default:
			{
				LEDs_FlashLEDs(LED_RED);
				Endpoint_ClearIN();
				Endpoint_ClearOUT();
				sei();
				return;
			}
				break;
		}
		LEDs_FlashLEDs(LED_BLUE);
		Endpoint_ClearIN();
		Endpoint_ClearOUT();
// 		_delay_ms(2000);
		sei();
	}
	return;
}

/** Turn servo */
void Turn_Servo(double angle)
{
	cli();
	DDRB |= PB1;
	PORTB &= ~PB1;
	
	int servo_pulses = 100;
	double servo_period_ms = 20;

// Choose between 180 or 360 degree servo
#if 0 // Use 1 here for 180 degree servo
	// For 180 degree servo, pulse width is 1.25ms for ~0 degrees and 1.75ms for ~180 degrees
 	double range_ms = 0.5;
 	double start_ms = 1.25;
 	double range_fraction = angle/180;
#else
	// For 360 degree servo, pulse width is 1.5ms for ~5 degrees and 2.5ms for ~355 degrees
	double range_ms = 1.0;
	double start_ms = 1.5;
	double range_fraction = angle/360;
#endif

	double time_high_ms = start_ms + range_fraction*range_ms;
	double time_low_ms = servo_period_ms - time_high_ms;
	
	for (int i=0; i<servo_pulses; i++)
	{
		PORTB |= PB1;
		_delay_ms(time_high_ms);
		PORTB &= ~PB1;
		_delay_ms(time_low_ms);
	}
	
	PORTB &= ~PB1;
	sei();
}

/** Start square wave output */
void Setup_FreqGen(double out_freq)
{
	// WRONG PIN... DDRC |= 0b01000000; //set OC.1A (Bit6 = 1) as output

	//set prescaler at min
	TCCR1B = (TCCR1B & 0b11111000) | 0b00000001;	//Bit2=0; Bit1=0; Bit0=1

	//set mode: Clear Timer on Compare (CTC) (WGM3:0 = 0100)
	// 	WGM10 = 0 (Bit0 = 0) ; WGM11 = 0 (Bit1 = 0)
	TCCR1A = TCCR1A & 0b11111100;

	// 	WGM2 = 1 (Bit3 = 1) ; WGM3 = 0 (Bit4 = 0)
	TCCR1B = (TCCR1B | 0b00000000) & 0b11101111;

	CLKPR = 0b10000000;	//enable changes on CLKPR register (Bit7 = 1)
	CLKPR = 0b00000000;	//set clock division factor to 1 (Bit3:0 = 0)

	//set COM1A mode to toggle pin (Bit7 = 0 ; Bit6 = 1)
	TCCR1A = (TCCR1A & 0b01111111) | 0b01000000;

// 	double out_freq = ;		// 122 Hz to 8 MHz
	double xtal_freq = 16e6;

	double out_period = 1.0/out_freq;
	double xtal_period = 1.0/xtal_freq;

	//calculate total number of cycles for half period
	long cycles = round(0.5 * out_period / xtal_period );

	//set 16 bit output compare register to number of cycles for half an output period
	OCR1A = (unsigned short int) cycles;
}

/** Start timer and counter */
void start_USB_timer(void)
{
	usb_timer_running = 1;
	//set mode: normal (WGM2:0 = 0010)
	TCCR0A = 0b00000000;
	TCCR0B = 0b00000000;
	
	//set clock prescaler to 1024
	TCCR0B = (TCCR1B | 0b00000101) & 0b11111101;
	
	//enable timer0 overflow interupt enable
	TIMSK0 |=  0b00000001;
}

/** Start timer and counter */
double Get_Temp(void)
{
	DDRC &= 0b01111111; //set ICP1 (Bit7 = 1) as input
	DDRC |= 0b01000000; //set PC6 (Bit6 = 1) as output 
	
	PORTC |= 0b01000000; // Turn on temperature sensor
	_delay_ms(50);
	
	//set mode: Normal (WGM3:0 = 0000)
	// WGM10 = 0 (Bit0 = 0) ; WGM11 = 0 (Bit1 = 0)
	TCCR1A = TCCR1A & 0b11111100;
	// WGM2 = 1 (Bit3 = 0) ; WGM3 = 0 (Bit4 = 0)
	TCCR1B = TCCR1B & 0b11100111;
	
	//set prescaler to 1
	TCCR1B = (TCCR1B & 0b11111000) | 0b00000001;	//Bit2=0; Bit1=0; Bit0=1

	// set input capture noise filter and edge select
	TCCR1B = TCCR1B | 0b11000000;
	//   TCCR1B &= 0b00111111;

	CLKPR = 0b10000000;	//enable changes on CLKPR register (Bit7 = 1)
	CLKPR = 0b00000000;	//set clock division factor to 1 (Bit3:0 = 0)
	
	count = 0;
	total = 0;
	period = 0;
	
	sei();
	
	TIMSK1 |= 0b00100000; // Input Capture Interrupt Enable
	
	double max_period_ms = 4;
	double max_time_ms = max_period_ms * (num_av + settle*2);
	
	_delay_ms(max_time_ms);
	
	if (count < num_av + settle) 
	{
		LEDs_FlashLEDs(LED_RED);
		_delay_ms(100);
		LEDs_FlashLEDs(LED_RED);
		_delay_ms(100);
		LEDs_FlashLEDs(LED_RED);
		return 999;
	}
	
	LEDs_FlashLEDs(LED_BLUE);
	
	double average = total / num_av;
	double period_s = average / 16e6;
	double max657_mult = 10.0;
	double s_to_us = 1e6;
	double deg_to_kelvin = -273.15;
	double temp = period_s*s_to_us / max657_mult + deg_to_kelvin;
	
	cli();
	
	return temp;
}

void Laptop_Switch(void)
{
	DDRC |= 0b00010000; // set PC4 (Bit4 = 1) as output 
	PORTC |= 0b00010000; 
	_delay_ms(800);
	PORTC &= ~0b00010000;
}
				
/* Interrupt routine */
ISR(TIMER1_CAPT_vect) 
{
	current = (ICR1H*256) + ICR1L;
	
	// Discard the first 10
	if (count > settle)
	{
		period = current - prev;
		total += period;
		
		if ( count > (num_av + settle - 1) )
		{
			// Turn off input capture interrupt
			TIMSK1 &= 0b11011111;
			return;
		}
	}
	prev = current;
	count++;
}

/* Interrupt routine */
ISR(TIMER0_OVF_vect) 
{
	usb_timer_value++;
	if (usb_timer_value > usb_timer_expire)
	{
		cli();
		//turn off overflow interupt
		TIMSK0 &=  ~0b00000001;
		usb_timer_value = 0;
		
		if (USB_DeviceState != DEVICE_STATE_Configured)
		{
			if (sleeping) return;
			LEDs_FlashLEDs(LED_RED);
			_delay_ms(100);
			LEDs_FlashLEDs(LED_RED);
			_delay_ms(100);
			LEDs_FlashLEDs(LED_RED);
			//power button
			Laptop_Switch();
			
		}
		else
		{
			LEDs_FlashLEDs(LED_BLUE);
			_delay_ms(100);
			LEDs_FlashLEDs(LED_BLUE);
			_delay_ms(100);
			LEDs_FlashLEDs(LED_BLUE);
		}
		sei();
	}
}
